/*
 * manager.cpp
 *
 *  Created on: 6 mai 2012
 *      Author: user
 */

#include <sstream>
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <image_transport/subscriber.h>
#include <image_transport/image_transport.h>
#include <unistd.h>

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

#include <cv_bridge/CvBridge.h>
#include <opencv2/highgui/highgui.hpp>
//#include "window_thread.h"

#include <boost/thread.hpp>
#include <boost/format.hpp>

#include <gtk/gtk.h>

#include "boost/thread/mutex.hpp"

//Includes for arrow
//
#include <math.h>


void imageCb               (const sensor_msgs::ImageConstPtr& msg);
void start_saving          (GtkWidget *widget, gpointer data);
void chooseDirectory       (GtkWidget *widget, gpointer data);
gboolean handleMouseMotion (GtkWidget *widget, GdkEventMotion *event, gpointer data);
gboolean handleMousePress  (GtkWidget *widget, GdkEventButton *event, gpointer data);
gboolean handleKeyPress    (GtkWidget *widget, GdkEventKey *event, gpointer data);
void exitProgram           (GtkWidget *widget, gpointer data);
void force_refresh         (const ros::TimerEvent&);
void drawArrow2            (float angle, float speed, IplImage* image, int width, int height);

static const char WINDOW_NAME[] = "Unicebot manager";

image_transport::Subscriber sub_;
sensor_msgs::CvBridge img_bridge_;

boost::mutex image_mutex_;
sensor_msgs::ImageConstPtr last_msg_;
cv::Mat last_image_;

std::string window_name_= WINDOW_NAME;
boost::format filename_format_;
std::string default_saving_folder_ = "./images/image_";
std::string default_saving_extension_ = ".jpg";
int count_;

GtkWidget *window;
GtkWidget *vbox;
GtkWidget *toolbar;
GtkWidget *imageWidget;
GtkWidget *fileChooser;

bool RECORD_ = false;
int nb_ = 1;

float angular_ = 0.0f;
float angular_coef =0.3f;
float linear_ = 0.0f;
float speedrate_ = 0.0f;
float max_linear_=0.5f;
float pas_ = 0.05f;
int padding_ = 20;

int callCounter = 0;
bool move_ = false;

//TOPICS
std::string transport_ = "compressed";
std::string topic1_ = "/camera/rgb/image_color";
std::string topic2_ = "/camera/depth/image";
std::string topic3_ = "/camera/rgb/image_mono";
std::string activatedTopic_ = topic1_;

ros::Publisher vel_pub_;
boost::mutex publish_mutex_;

int main(int argc, char **argv) {
	ros::init(argc, argv, "manager", ros::init_options::AnonymousName);

	ros::NodeHandle n;

	// INIT WINDOW
	g_thread_init(NULL);
	gdk_threads_init();
	gtk_init(&argc, &argv);

	window = gtk_window_new(GTK_WINDOW_TOPLEVEL);
	gtk_window_set_position(GTK_WINDOW(window), GTK_WIN_POS_CENTER);
	gtk_window_set_default_size(GTK_WINDOW(window), 250, 200);
	gtk_window_set_title(GTK_WINDOW(window), window_name_.c_str());

	vbox = gtk_vbox_new(FALSE, 0);
	gtk_container_add(GTK_CONTAINER(window), vbox);

	// MENUBAR
	GtkWidget *menubar;
	GtkWidget *filemenu;
	GtkWidget *file;
	GtkWidget *quit;

	menubar = gtk_menu_bar_new();
	filemenu = gtk_menu_new();

	file = gtk_menu_item_new_with_label("Fichier");
	quit = gtk_menu_item_new_with_label("Quitter");

	gtk_menu_item_set_submenu(GTK_MENU_ITEM(file), filemenu);
	gtk_menu_shell_append(GTK_MENU_SHELL(filemenu), quit);
	gtk_menu_shell_append(GTK_MENU_SHELL(menubar), file);
	gtk_box_pack_start(GTK_BOX(vbox), menubar, FALSE, FALSE, 3);

	g_signal_connect(G_OBJECT(quit), "activate",
	G_CALLBACK(gtk_main_quit), NULL);

	// TOOLBAR
	GtkToolItem *newa;
	GtkToolItem *save;
	GtkToolItem *sep;
	GtkToolItem *exit;

	toolbar = gtk_toolbar_new();
	gtk_toolbar_set_style(GTK_TOOLBAR(toolbar), GTK_TOOLBAR_ICONS);

	gtk_container_set_border_width(GTK_CONTAINER(toolbar), 2);

	newa = gtk_tool_button_new_from_stock(GTK_STOCK_OPEN);
	gtk_toolbar_insert(GTK_TOOLBAR(toolbar), newa, -1);
	g_signal_connect(G_OBJECT(newa), "clicked",
		G_CALLBACK(chooseDirectory), NULL);

	save = gtk_tool_button_new_from_stock(GTK_STOCK_SAVE);
	gtk_toolbar_insert(GTK_TOOLBAR(toolbar), save, -1);
	g_signal_connect(G_OBJECT(save), "clicked",
		G_CALLBACK(start_saving), NULL);

	sep = gtk_separator_tool_item_new();
	gtk_toolbar_insert(GTK_TOOLBAR(toolbar), sep, -1);

	exit = gtk_tool_button_new_from_stock(GTK_STOCK_QUIT);
	gtk_toolbar_insert(GTK_TOOLBAR(toolbar), exit, -1);

	gtk_box_pack_start(GTK_BOX(vbox), toolbar, FALSE, FALSE, 5);

	g_signal_connect(G_OBJECT(exit), "clicked",
	 	G_CALLBACK(gtk_main_quit), NULL);

	// ADD MOUSE MOVE EVENT HANDLING
	gtk_widget_set_events(window, GDK_POINTER_MOTION_HINT_MASK | GDK_POINTER_MOTION_MASK
									 | GDK_BUTTON_PRESS_MASK | GDK_BUTTON_RELEASE_MASK | GDK_KEY_PRESS_MASK | GDK_KEY_RELEASE_MASK );
	g_signal_connect(G_OBJECT(window), "motion-notify-event",
	 	G_CALLBACK(handleMouseMotion), NULL);

	g_signal_connect(G_OBJECT(window), "button-press-event",
	 	G_CALLBACK(handleMousePress), NULL);

	g_signal_connect(G_OBJECT(window), "button-release-event",
	 	G_CALLBACK(handleMousePress), NULL);

	// ADD KEY PRESS EVENT HANDLING
	g_signal_connect(G_OBJECT(window), "key-press-event",
	 	G_CALLBACK(handleKeyPress), NULL);

	g_signal_connect(G_OBJECT(window), "key-release-event",
	 	G_CALLBACK(handleKeyPress), NULL);

	//gtk_widget_set_events(window, GDK_POINTER_MOTION_HINT_MASK);

	g_signal_connect_swapped(G_OBJECT(window), "destroy",
		G_CALLBACK(exitProgram), NULL);

	gtk_widget_show_all(window);

	// CREATE ROS TIMER
	ros::Timer timer = n.createTimer(ros::Duration(0.1), force_refresh);

	// Subscribe to ros topic
	image_transport::ImageTransport it(n);
	sub_ = it.subscribe(activatedTopic_, 1, &imageCb, transport_);
	
	// Create publisher
	vel_pub_ =  n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
	
	ros::MultiThreadedSpinner spinner(4);
	spinner.spin();

	gdk_threads_enter();
	gtk_main();
	
	gdk_threads_leave();

	return 0;
}

void force_refresh(const ros::TimerEvent&) {
	boost::mutex::scoped_lock lock(publish_mutex_);
	geometry_msgs::Twist vel;
	vel.angular.z = angular_;
	vel.linear.x = linear_;
	vel_pub_.publish(vel);
}

gboolean handleKeyPress(GtkWidget *widget, GdkEventKey *event, gpointer data) {
	if(imageWidget != NULL) {
		if(event->type == GDK_KEY_PRESS) {
			// LEFT
			if(event->keyval == 65361 && move_) {
				angular_ = 1.50;
			}
			// RIGHT
			else if(event->keyval == 65363 && move_) {
				angular_ = -1.50;
			}
			// TOP
			else if(event->keyval == 65362 && move_) {
				linear_ = 0.5 * speedrate_;
			}
			// BOTTOM
			else if(event->keyval == 65364 && move_) {
				linear_ = -0.5 * speedrate_;
			}
			else if(event->keyval == 65364 && move_) {
				linear_ = -0.5 * speedrate_;
			}
			// R (start record)
			else if(event->keyval == 0x072)
			{
				RECORD_ = !RECORD_;
			}
			// SPACE
			else if(event->keyval == 0x020)
			{
				move_ = !move_;
				linear_ = 0.0;
				angular_ = 0.0;
			}
			//P
			else if(event->keyval == 0x070 && move_)
			{
				ros::NodeHandle n;
				image_transport::ImageTransport it(n);

				if(activatedTopic_.compare(topic1_)==0)
				{
					activatedTopic_ = topic2_;
				}
				else if(activatedTopic_.compare(topic2_)==0)
				{
					activatedTopic_ = topic3_;
				}
				else if(activatedTopic_.compare(topic3_)==0)
				{
					activatedTopic_ = topic1_;
				}
				
				sub_ = it.subscribe(activatedTopic_, 1, &imageCb, transport_);
			}
		}
		else if(event->type == GDK_KEY_RELEASE && move_) {
			linear_ = 0.0;
			angular_ = 0.0;
		}

		boost::mutex::scoped_lock lock(publish_mutex_);
		geometry_msgs::Twist vel;
		vel.angular.z = angular_;
  		vel.linear.x = linear_;
		vel_pub_.publish(vel);
	}

	return TRUE;
}

gboolean handleMouseMotion(GtkWidget *widget, GdkEventMotion *event, gpointer data) {

	if(imageWidget != NULL && move_) {
		gdouble xImgWidgetpos   = imageWidget->allocation.x;
		gdouble yImgWidgetpos   = imageWidget->allocation.y;
		gdouble imgWidgetWidth  = imageWidget->allocation.width;
		gdouble imgWidgetHeight = imageWidget->allocation.height;

		if(event->x >= xImgWidgetpos && event->x <= (imgWidgetWidth + xImgWidgetpos)) {
			if(event->y >= yImgWidgetpos && event->y <= (imgWidgetHeight + yImgWidgetpos)) {
				//calcul de la vitesse de rotation par rapport à la position de la souris
				//
				int left_max_   = xImgWidgetpos + padding_;
				int right_max_  = (imgWidgetWidth + xImgWidgetpos) - padding_;
				int left_zero_  = (imgWidgetWidth + xImgWidgetpos) / 2 - padding_;
				int right_zero_ = (imgWidgetWidth + xImgWidgetpos) / 2 + padding_;

				//souris au centre
				if(event->x >= left_zero_ && event->x<= right_zero_)
				{
					angular_ = 0.0f;
				}

				//souris au mayimum à droit ou à gauche
				else if(event->x < left_max_ || event->x > right_max_)
				{
					if(event->x < left_max_)
					{
						//gauche
						angular_ = 1.5f;
					}
					else
					{
						//droite
						angular_ = -1.5f;
					}
				}

				//souris côté gauche entre le centre et le may
				else if(event->x>=left_max_ && event->x<left_zero_)
				{
					angular_ = angular_coef *(1.0f - (((event->x - left_max_)*1.0f) / ((left_zero_ - left_max_)*1.0f))) *1.5f;
				}

				//souris côté droit entre le centre et le may
				else if(event->x<right_max_ && event->x>=right_zero_)
				{
					angular_ = angular_coef* (((event->x - right_zero_)*1.0f) / ((right_max_ - right_zero_)*1.0f)) *-1.5f;
				}

				boost::mutex::scoped_lock lock(publish_mutex_);
				geometry_msgs::Twist vel;
				vel.angular.z = angular_;
  				vel.linear.x = linear_;
				vel_pub_.publish(vel);
			}
			else {
				angular_ = 0;
				linear_ = 0;
			}
		}
		else {
			angular_ = 0;
			linear_ = 0;
		}
	}
	return true;
}

gboolean handleMousePress  (GtkWidget *widget, GdkEventButton *event, gpointer data) {
if(imageWidget != NULL && move_) {
		gdouble xImgWidgetpos  = imageWidget->allocation.x;
		gdouble yImgWidgetpos   = imageWidget->allocation.y;
		gdouble imgWidgetWidth  = imageWidget->allocation.width;
		gdouble imgWidgetHeight = imageWidget->allocation.height;

		if(event->x >= xImgWidgetpos && event->x <= (imgWidgetWidth + xImgWidgetpos)) {
			if(event->y >= yImgWidgetpos && event->y <= (imgWidgetHeight + yImgWidgetpos)) {
				if(event->type == GDK_BUTTON_PRESS) {
					// bouton gauche
					if(event->button == 1) {
						 
						linear_ += pas_;
	
					}
					// Bouton milieu
					else if(event->button == 2) {
						angular_ = 0.0f;
						linear_ = 0.0f;
					}
					// Bouton droit
					else if(event->button == 3) {
						 
						linear_ -= pas_;
					}
					//Molette en avant
					else if(event->button == 4) {
						 
						linear_ -= pas_;
					}
					// molette en arriere
					else if(event->button == 5) {
						 
						linear_ += pas_;
					}
					// Borner la vitesse
					//
					if(linear_ > max_linear_) { linear_ = max_linear_;}
					else if(linear_ < -max_linear_){ linear_ =-max_linear_;}

					/*
					std::ostringstream oss; 
					oss << "Vitesse: " << linear_;
					ROS_WARN(oss.str().c_str());
					//*/
				}

				boost::mutex::scoped_lock lock(publish_mutex_);
				geometry_msgs::Twist vel;
				vel.angular.z = angular_;
  				vel.linear.x = linear_;
				vel_pub_.publish(vel);
			}
			else {
				angular_ = 0;
				linear_ = 0;
			}
		}
		else {
			angular_ = 0;
			linear_ = 0;
		}
	}
	return TRUE;
}

void start_saving(GtkWidget *widget, gpointer data) {
	RECORD_ = !RECORD_;
}

void chooseDirectory(GtkWidget *widget, gpointer data) {
	// FILE CHOOSER
	fileChooser = gtk_file_chooser_dialog_new("Sélection d'un répertoire d'enregistrement", NULL, 
                                         GTK_FILE_CHOOSER_ACTION_SELECT_FOLDER, 
                                         GTK_STOCK_CANCEL, GTK_RESPONSE_CANCEL, 
                                         GTK_STOCK_OPEN, GTK_RESPONSE_ACCEPT, 
                                         NULL); 
	//gtk_file_chooser_set_filename(GTK_FILE_CHOOSER(fileChooser), default_saving_folder_.c_str()); // Default folder
	if (gtk_dialog_run (GTK_DIALOG (fileChooser)) == GTK_RESPONSE_ACCEPT) {
		char *filename;

		filename = gtk_file_chooser_get_filename (GTK_FILE_CHOOSER (fileChooser));

		// concat with /image_
		std::ostringstream oss; 
		std::string s1 = filename;
		std::string s2 = "/image_"; 
		oss << s1 << s2;		


		default_saving_folder_ = oss.str();
		g_free (filename);
	}

	//ROS_WARN(default_saving_folder_.c_str());
	gtk_widget_destroy(fileChooser);
	
	
}

//////////////////////////////////////////////////////////////////////////


void imageCb(const sensor_msgs::ImageConstPtr& msg) {
	image_mutex_.lock();

	// May want to view raw bayer data, which CvBridge doesn't know about
	if (msg->encoding.find("bayer") != std::string::npos) {
    		last_image_ = cv::Mat(msg->height, msg->width, CV_8UC1, 
									const_cast<uint8_t*>(&msg->data[0]), msg->step);
	}
	// We want to scale floating point images so that they display nicely
	else if(msg->encoding.find("F") != std::string::npos) {
		//ROS_WARN("####Floating:");

		cv::Mat float_image_bridge = img_bridge_.imgMsgToCv(msg, "32FC1");
		cv::Mat_<float> float_image = float_image_bridge;
		float max_val = 0;

		for(int i = 0; i < float_image.rows; ++i) {
			for(int j = 0; j < float_image.cols; ++j) {
				max_val = std::max(max_val, float_image(i, j));
			}
		}

		if(max_val > 0) {
			float_image /= max_val;
		}
		last_image_ = float_image;
	}
	else {
		// Convert to OpenCV native BGR color
		try {
			last_image_ = img_bridge_.imgMsgToCv(msg, "bgr8");

		}
		catch (sensor_msgs::CvBridgeException& e) {
			ROS_ERROR("Unable to convert '%s' image to bgr8", msg->encoding.c_str());
		}
	}


  	// conversion
	if (!last_image_.empty()){
		IplImage* image = new IplImage(last_image_);

		// RECORDING KINECT IMAGES
		if(RECORD_ == true)
		{
			std::ostringstream oss; 
			std::string s1 = default_saving_folder_.c_str();
			std::string s2 = default_saving_extension_.c_str(); 
			oss << s1 << nb_ << s2;

			nb_++;	
			cvSaveImage(oss.str().c_str() ,image);
		}

		//Add arrow
		//
		/*IplImage* image = */drawArrow2(angular_, linear_, image, msg->width, msg->height);

		// REFRESHING KINECT IMAGE DISPLAY
		gdk_threads_enter();
		GdkPixbuf *pixbuf;
	
		//Create another image with the same stuff
		IplImage *gtkMask = cvCreateImage(cvSize(image->width, image->height), 
										image->depth, image->nChannels);
		cvCvtColor(image, gtkMask, CV_BGR2RGB);//Copy the IplImage to RGB format
		image_mutex_.unlock();
		//Copy the IplImage data to a GdkPixbuf:
		pixbuf = gdk_pixbuf_new_from_data (
		        (guchar*)gtkMask->imageData,//Raw image data
		        GDK_COLORSPACE_RGB,                     //Colorspace
		        FALSE,                                  //Has alpha channel or not
		        gtkMask->depth,                         //Bits per pixel
		        gtkMask->width,                         //Width of image
		        gtkMask->height,                        //Height of image
		        gtkMask->widthStep,                     //Row stride
		        NULL,                                   //Not sure what this is
		        NULL                                    //Not sure what this is
		);

		if(callCounter == 0) {
		  	imageWidget = gtk_image_new_from_pixbuf(pixbuf);
		  	gtk_box_pack_start(GTK_BOX(vbox), imageWidget, TRUE, TRUE, 5);
	  		callCounter++;
		}
		else {
			gtk_image_set_from_pixbuf(GTK_IMAGE(imageWidget), pixbuf);
		}

		gtk_widget_show_all(window);

		gdk_flush();
		while(gtk_events_pending())
			  gtk_main_iteration();
		gdk_flush();

		gdk_threads_leave();	

		if(gtkMask != NULL) {
			delete(image);
			cvReleaseImage(&gtkMask);
			delete(gtkMask);
		}
		if(pixbuf != NULL) {
			g_object_unref(pixbuf);
		}
		last_image_.release();

	}

}

void exitProgram (GtkWidget *widget, gpointer data) {
		boost::mutex::scoped_lock lock(publish_mutex_);
		geometry_msgs::Twist vel;
		vel.angular.z = 0.0;
		vel.linear.x = 0.0;
		vel_pub_.publish(vel);
}

void drawArrow2(float angle, float speed, IplImage* image, int width, int height)
{
	if(RECORD_) 
	{
			cvCircle(image,                       /* the dest image */
				 cvPoint(15, 15), 1,     			  /* center point and radius */
				 cvScalar(0, 0, 255, 0),    		  /* the color; red */
				      5, 8, 0);                    /* thickness, line type, shift */
	}
	if(speed!=0 && speed < 0)
	{
		speed = speed * (-1);
		if(angle < 0){
			angle = -M_PI-angle;
		}
		else{
			angle = M_PI-angle;
		}
	}
	else if(speed == 0 && angle!=0) 
	{
		speed = angle/3.0;
		if(angle < 0){
			angle = -M_PI/2.0 + M_PI;
		}
		else{
			angle = M_PI/2.0;
		}
	}

	if(speed != 0.0 || angle!= 0.0){
		
		// determination de la longueur selon la vitesse
		int length = (int)floor(speed*100);
		int length2 = (int)floor(speed*90);

		// origine de l'image (centre)
		CvPoint* origine = new CvPoint();
		origine->x = (int)width/2;
		origine->y = (int)height/2;


		// coordonnees cartesiennes de l'extremite deduites de ses coordonnees polaires (vitesse traduite en distance et angle en radians)
		int x = (int)floor(length*cos(angle));
		int y = (int)floor(length*sin(angle));

		// Adaptation des coordonnees au referentiel de l'image
		// Premiere extremite de la droite
		CvPoint* e1 = new CvPoint();
		e1->x = origine->x-y;
		e1->y = origine->y-x;

		// Seconde extremite de la droite
		//~ cvPoint e2 = cvPoint(200+y, 150+x);
		CvPoint* e2 = new CvPoint();
		e2->x = origine->x+y;
		e2->y = origine->y+x;

		// Tete de fleche
		// droite
		int x1 = (int)floor(length2*cos(angle-0.314));
		int y1 = (int)floor(length2*sin(angle-0.314));
		CvPoint* ed = new CvPoint();
		ed->x = origine->x-y1;
		ed->y = origine->y-x1;
		// gauche
		int x2 = (int)floor(length2*cos(angle+0.314));
		int y2 = (int)floor(length2*sin(angle+0.314));
		CvPoint* eg = new CvPoint();
		eg->x = origine->x-y2;
		eg->y = origine->y-x2;


		/* draw the main line */
		cvLine(image,                  /* the dest image */
				*origine,          	     /* start point */
				*e1,           		     /* end point */
				cvScalar(0, 255, 0, 0),      /* the color; red */
				5, 8, 0);                    /* thickness, line type, shift */

		/* draw the right arrow line */
		cvLine(image,                  /* the dest image */
				*e1,          	     	 /* start point */
				*ed,           		     /* end point */
				cvScalar(0, 255, 0, 0),      /* the color; red */
				5, 8, 0);                    /* thickness, line type, shift */

		/* draw the left arrow line */
		cvLine(image,                  /* the dest image */
				*e1,          	     	 /* start point */
				*eg,           		     /* end point */
				cvScalar(0, 255, 0, 0),      /* the color; red */
				5, 8, 0);                    /* thickness, line type, shift */

		/* draw the opposite line */
		cvLine(image,                  /* the dest image */
				*origine,          	     /* start point */
				*e2,           		     /* end point */
				cvScalar(0, 255, 0, 0),      /* the color; green */
				5, 8, 0);                    /* thickness, line type, shift */
	}
	else{

			// vitesse nulle => surplace => dessin d'un cercle

			/* draw a green circle */
			cvCircle(image,                       /* the dest image */
				      cvPoint((int)width/2, (int)height/2), padding_+5,     /* center point and radius */
				      cvScalar(0, 0, 255, 0),    /* the color; red */
				      5, 8, 0);                  /* thickness, line type, shift */


	}

}
